﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using MathNet.Numerics.LinearAlgebra.Generic;
using MathNet.Numerics.LinearAlgebra.Double;
using GestureSaver;
using Microsoft.Kinect;


namespace CubeRotation
{
    public static class Rectificater
    {
        static Point XAxis = new Point(1, 0, 0);
        static Point YAxis = new Point(0, 1, 0); 
        static Point ZAxis = new Point(0, 0, 1);

        public static void Rectificate(Cube c)
        {
            Point v1 = c[1] - c[0];
            Point v2 = c[0] - c[2];
            Point v3 = c[0] - c[4];

            Matrix<double> m = new DenseMatrix(3, 3);
            
            m[0, 0] = v1.X;
            m[0, 1] = v1.Y;
            m[0, 2] = v1.Z;
            m[1, 0] = v2.X;
            m[1, 1] = v2.Y;
            m[1, 2] = v2.Z;
            m[2, 0] = v3.X;
            m[2, 1] = v3.Y;
            m[2, 2] = v3.Z;

            double angle1 = Math.Atan2(m[2, 0], m[2, 1]);
            double angle2 = Math.Acos(m[2, 2]);
            double angle3 = -Math.Atan2(m[0, 2],m[1,2]);

            //c.RotateZ(angle1);
            //c.RotateX(angle2);
            //c.RotateZ(angle3);
            
            for (int i = 0; i < 8; i++)
            {
                Vector<double> temp = new DenseVector(new double[] { c[i].X, c[i].Y, c[i].Z });
                Vector<double> result = m * temp;
                c[i].X = result[0];
                c[i].Y = result[1]; 
                c[i].Z = result[2];
            }
            c.Print();
        }

        public static void Rectificate(GestureSaver.Gesture g)
        {
            Matrix<double> m = new DenseMatrix(3, 3);         

            foreach (MSkeleton s in g)
            {
                MPosition shoulderLeft = s[JointType.ShoulderLeft].Position;
                MPosition shoulderRight = s[JointType.ShoulderRight].Position;
                MPosition shoulderCenter = s[JointType.ShoulderCenter].Position;
                MPosition hipCenter = s[JointType.HipCenter].Position;

                Point v1 = GetVector(shoulderRight, shoulderLeft);
                Point v2 = GetVector(shoulderCenter, hipCenter);
                Point v3 = CrossProduct(v1, v2);

                m[0, 0] += v1.X;
                m[0, 1] += v1.Y;
                m[0, 2] += v1.Z;
                m[1, 0] += v2.X;
                m[1, 1] += v2.Y;
                m[1, 2] += v2.Z;
                m[2, 0] += v3.X;
                m[2, 1] += v3.Y;
                m[2, 2] += v3.Z;                
            }

            m = m.Divide(g.SkeletonCount);

            // for all skeletons...
            foreach (MSkeleton s in g)
            {            
                // double angle1 = Math.Atan2(m[2, 0], m[2, 1]);
                // double angle2 = Math.Acos(m[2, 2]);
                // double angle3 = -Math.Atan2(m[0, 2], m[1, 2]);
                
                // Rotation order, if needed: (Z,Angle1),(X,Angle2),(Z,Angle3) [Not used.]

                foreach (JointType jType in Enum.GetValues(typeof(JointType)))
                {
                    MPosition oldPos = s[jType].Position;

                    Vector<double> temp = new DenseVector(new double[] { oldPos.X, oldPos.Y, oldPos.Z });
                    Vector<double> result = m * temp;

                    MPosition newPos = new MPosition() { X = (float)result[0], Y = (float)result[1], Z = (float)result[2] };

                    s[jType].Position = newPos;
                }
            }
        }

        private static Point GetVector(MPosition p1, MPosition p2)
        {
            return new Point(p1.X - p2.X, p1.Y - p2.Y, p1.Z - p2.Z);
        }

        private static Point CrossProduct(Point a, Point b)
        {
            Matrix<double> m = new DenseMatrix(3, 3);

            m[0, 0] = 0;
            m[0, 1] = -a.Z;
            m[0, 2] = a.Y;
            m[1, 0] = a.Z;
            m[1, 1] = 0;
            m[1, 2] = -a.X;
            m[2, 0] = -a.Y;
            m[2, 1] = a.X;
            m[2, 2] = 0;

            Vector<double> v = b.V();

            Vector<double> result = m * v;

            return result.ToPoint();

        }
    }
}
